#include <spline.hpp>
namespace ohe_framework
{

Spline::Spline(int point)
{
    size = point;
    grad_height = new CircularBuffer(size);
    grad_stagger = new  CircularBuffer(size);
    grad2_height = new CircularBuffer(size);
    grad2_stagger = new  CircularBuffer(size);
    heights =new  CircularBuffer(size);
    staggers =new  CircularBuffer(size);
    inputs = new CircularBuffer(size);
    diff_input = new CircularBuffer(size);
    points = new CircularBuffer(size);
    s1 = new float[size];
    s2 = new float[size];
    m1 = new float*[size];
    m2 = new float*[size];
    for(int i=0;i<size;i++)
    {
        m1[i]=new float[size];
        m2[i]=new float[size];
    }
    for(int i=0;i<size;i++)
        for(int j=0;j<size;j++)
        {
            m1[i][j]=0;
            m2[i][j]=0;
        }
    count =0 ;
}

void Spline::addPoint(float x, float height, float stagger, int point)
{
    inputs->write(x);
    heights->write(height);
    staggers->write(stagger);
    points->write(point);
    int i = points->getWriteIndex();
    diff_input->write((inputs->read(i)-inputs->read(i-1)));
    grad_height->write((heights->read(i)-heights->read(i-1))/diff_input->read(i));
    grad_stagger->write((staggers->read(i)-staggers->read(i-1))/diff_input->read(i));
    if(count>2)
    {
        acce_height = (6.0*(grad_height->read(i)-grad_height->read(i-1))-diff_input->read(i)*acce_height)/
                (2*(diff_input->read(i)+diff_input->read(i-1)));
        acce_stagger = (6.0*(grad_stagger->read(i)-grad_stagger->read(i-1))-diff_input->read(i)*acce_stagger)/
                (2*(diff_input->read(i)+diff_input->read(i-1)));
    }
    else
    {
        acce_height = 0;
        acce_stagger = 0;
        count++;
    }
//    std::cout<<i<<"\t"<<inputs->read(i)<<"\t"<<heights->read(i)<<"\t"<<staggers->read(i)<<"\t"
//            <<points->read(i)<<"\t"<<diff_input->read(i)<<"\t"<<grad_height->read(i)<<"\t"
//           <<grad_stagger->read(i)<<std::endl;
//    reCalculateMatrixes();
//    forwardCalculation();
//    backwordCalculation();
}

void Spline::reCalculateMatrixes()
{
    int j = points->getWriteIndex();
    //std::cout<<j<<std::endl;
    
    for(int i=1;i<(size-1);i++)
    {
        //std::cout<<i<<"\t"<<j<<"\t"<<size<<std::endl;
        float buf = diff_input->read(i+j);
        float buf1 = diff_input->read(i+j+1);
        m2[i][i]=(buf+buf1)*2.0;
        m1[i][i]=(buf+buf1)*2.0;
        if(i!=1)
        {
            m1[i][i-1]=buf;//=m1[i][i-1];//=diff_input->read(i+j);
            m2[i][i-1]=buf;
            m1[i-1][i]=buf;
            m2[i-1][i]=buf;
        }
        buf = grad_height->read(i+j+1);
        buf1 = grad_height->read(i+j+2);
        m1[i][size-1]=(float)(buf1-buf)*6.0;
        m2[i][size-1]=(float)(buf1-buf)*6.0;
    }
}

void Spline::forwardCalculation()
{
    for(int i=1;i<size-2;i++)
       {
           float temp1=(m1[i+1][i]/m1[i][i]);
           float temp2=(m2[i+1][i]/m2[i][i]);
           for(int j=1;j<=size-1;j++)
           {
               m1[i+1][j]-=temp1*m1[i][j];
               m2[i+1][j]-=temp2*m2[i][j];
           }
       }
}

void Spline::backwordCalculation()
{
    for(int i=size-2;i>0;i--)
    {
        float sum1=0;
        float sum2=0;
        for(int j=i;j<=size-2;j++)
        {
            sum1+=m1[i][j]*s1[j];
            sum2+=m2[i][j]*s2[j];
        }
        s1[i]=(m1[i][size-1]-sum1)/m1[i][i];
        s2[i]=(m2[i][size-1]-sum2)/m2[i][i];
        //std::cout<<s1[i]<<"\t"<<s2[i]<<std::endl;
    }
}

void Spline::getPoints(float *&height_points,float *&stagger_points,int ref)
{
    //reCalculateMatrixes();
    //forwardCalculation();
    //backwordCalculation();
    int j = points->getWriteIndex();
    int no_points = (int)(points->read(j))-ref;
    int size_ = size-ref;
    ////////////////////////////////////////////////////////////////////////////////
    float x1 = inputs->read(j)-inputs->read(j-1);
    ////////////////////////////////////////////////////////////////////////////////
    float a1=(0-acce_height)/(6*diff_input->read(j-1));
    float b1=acce_height/2;
    float c1=(heights->read(j)-heights->read(j-1))/diff_input->read(j-1)
            -(2*diff_input->read(j-1)*acce_height+0*diff_input->read(j-1))/6;
    float d1=heights->read(j-1);
    /////////////////////////////////////////////////////////////////////////////////
    float a2=(0-acce_stagger)/(6*diff_input->read(j-1));
    float b2=acce_stagger/2;
    float c2=(staggers->read(j)-staggers->read(j-1))/diff_input->read(j-1)
            -(2*diff_input->read(j-1)*acce_stagger+0*diff_input->read(j-1))/6;
    float d2=staggers->read(j-1);
    //////////////////////////////////////////////////////////////////////////////////
//    std::cout<<x1<<"\t"<<a1<<"\t"<<b1<<"\t"<<c1<<"\t"<<d1<<
//               "\t"<<a2<<"\t"<<b2<<"\t"<<c2<<"\t"<<d2<<"\t"<<j<<std::endl;    
    /////////////////////////////////////////////////////////////////////////////////
    for(int i=1;i<(no_points)+1;i++)
    {
        float p = x1-i*0.05;
        height_points[i]=a1*p*p*p+b1*p*p+c1*(p)+d1;
        stagger_points[i]=a2*p*p*p+b2*p*p+c2*(p)+d2;
    }
}

void Spline::clear()
{
    for(int i=0;i<size;i++)
    {
        grad_height->write(0);
        grad_stagger->write(0);
        diff_input->write(0);
        heights->write(0);
        staggers->write(0);
        points->write(0);
        inputs->write(0);
    }
}

}// end namespace ohe_framework

